#include <iostream>
#include <queue>
#include <list>
#include <functional>
#include <algorithm>
#include <assert.h>

using namespace std;


void logMsg(const string &msg)
{
	cout<<msg.c_str()<<endl;
}


struct AStarNode 
{
	int index;
	int x, y;
	float g, h, f;
	int state;		// 0:can walk 1:wall ....

	AStarNode *parent;

	AStarNode(void) : index(0), x(0), y(0), g(0.0), h(0.0), f(0.0), state(0) ,parent(0){}

	~AStarNode(void) {}

	bool operator==(const AStarNode& k) { return ((x == k.x) && (y == k.y)); }
	bool operator<(const AStarNode& k) { return f > k.f; }

	bool canMove(void) { return (state == 0); }
};

bool compare_data(const AStarNode *nodea, const AStarNode *nodeb)
{
	return nodea->f < nodeb->f;
}


/**
	注意x,y都是从0开始的

	AStar tmpAStar;
	tmpAStar.buildMap(10, 10);
	tmpAStar.setInitNode(5, 5, 10, 10);
	tmpAStar.addUnableNode(3, 5);	// 添加障碍
	....
	tmpAStar.startAStar();

*/
class AStar
{
protected:
	typedef vector<AStarNode*>	AStarVect;
	typedef list<AStarNode*>	AStarList;
	// map	use a array to do this
	AStarVect mMapVect;
	// open list
	AStarList mOpenList;
	// close list
	AStarList	mCloseList;
	// 最后的结果
	AStarVect mFinishVect;

	AStarNode	*mStartNode, *mEndNode;

	int miWidth, miHeight;
public:
	AStar(void){}
	~AStar(void)
	{
		AStarVect::iterator iter = mMapVect.begin();
		for (; iter != mMapVect.end(); ++iter)
		{
			delete (*iter);
		}
		mMapVect.clear();
	}

	void test1()
	{
		logMsg("==== start test ===");
		//tmpAStar.buildMap(10, 10);
		//tmpAStar.setInitNode(5, 5, 10, 10);
		//tmpAStar.addUnableNode(3, 5);	// 添加障碍
		//....
		//	tmpAStar.startAStar();
		// 说明使用的算法
		buildMap(8, 8);
		setInitNode(1, 2, 5, 2);
		addUnableNode(3, 1);
		addUnableNode(3, 2);
		addUnableNode(3, 3);
		startAStar();
		//setInitNode()
		logMsg("==== end test ===");
	}

	//-----------------------------------------------------------------------------
	//
	void buildMap(int x, int y)
	{
		logMsg("==== buidMap ===");
		miWidth = x;
		miHeight = y;
		for (int i = 0; i < (x * y); i++)
		{
			AStarNode *tmpNode = new AStarNode;
			tmpNode->parent = NULL;
			tmpNode->index = i;
			tmpNode->x = i %  miWidth;
			tmpNode->y = i / miHeight;
			mMapVect.push_back(tmpNode);
		}
	}

	//-----------------------------------------------------------------------------
	//
	void setInitNode(int x0, int y0, int x1, int y1)
	{
		logMsg("==== InitNode ===");
		if (isValid(x0, y0) && isValid(x1, y1))
		{
			mStartNode = getNode(x0, y0);
			mEndNode = getNode(x1, y1);

			mOpenList.push_back(mStartNode);
		}
	}
	
	/** 
	* @brief addUnableNode  添加不可行节点
	* 
	* Detailed description.
	* @param[in] x 
	* @param[in] y 
	* @param[in] state Defaults to 1.
	*/
	void addUnableNode(int x, int y, int state = 1)
	{
		logMsg("==== addUnableNode ===");
		if (isValid(x, y))
		{
			int index = getIndex(x, y);
			mMapVect[index]->state = state;
		}
	}

	//-----------------------------------------------------------------------------
	//
	void startAStar(void)
	{
		while (mOpenList.size() > 0)
		{
			AStarNode *tmpNext = findMinF();
			addToCloseList(tmpNext);

			cout<<"next pos (" << tmpNext->x <<"," << tmpNext->y << ")"<<endl;
			if (tmpNext == mEndNode)
			{
				AStarNode * tmpNode = tmpNext;
				cout<<" find road"<<endl;
				cout<<" ========= road ====="<<endl;
				while (tmpNode != NULL)
				{
					mFinishVect.push_back(tmpNode);
					cout<<"("<<tmpNode->x<<","<<tmpNode->y<<")";
					tmpNode = tmpNode->parent;
				}
				cout<<" ========= road ====="<<endl;
				
				break;
			}

			AStarList tmpNearNodes;
			findNearNode(tmpNearNodes, tmpNext);
			AStarList tmpInList, tmpUnInList;
			findInOpenListAndUnInOpenList(tmpNearNodes, tmpInList, tmpUnInList);
			addToOpenList(tmpUnInList);
			calculateF(tmpNext, tmpUnInList);
		}
	}

	/** 
	* @brief findMinF 返回最小的F值的元素
	* 
	* Detailed description.
	* @return AStarNode *  
	*/
	AStarNode *findMinF(void)
	{
		//sort(mOpenList.begin(), mOpenList.end());	// 升序
		mOpenList.sort(compare_data);

		AStarNode *tmpFront = mOpenList.front();
		
		mOpenList.pop_front();

		return tmpFront;
	}

	/** 
	* @brief canUse 注意这里的x,y都是从0开始, 判断条件是不在开启列表和关闭列表中，并且有效的
	* 
	* Detailed description.
	* @param[in] x 
	* @param[in] y 
	* @return bool  
	*/
	bool canUse(int x, int y)
	{
		if (isValid(x, y))
		{
			int index = getIndex(x, y);
			AStarNode * tmpNode = mMapVect[index];
			if (!inOpenList(tmpNode) && !inCloseList(tmpNode))
			return (tmpNode->state == 0);
		}
		return false;
	}

	/** 
	* @brief isValid 判断当前点是否在map中，也就是说是否在地图上
	* 
	* Detailed description.
	* @param[in] x 
	* @param[in] y 
	* @return bool  
	*/
	bool isValid(int x, int y)
	{
		if (x < 0 || x >= miWidth || y < 0 || y >= miHeight)
			return false;

		return true;
	}
	
	
	/** 
	* @brief getIndex  获取列表中的索引
	* 
	* Detailed description.
	* @param[in] x 
	* @param[in] y 
	* @return int  
	*/
	int getIndex(int x, int y)
	{
		return  y * miWidth + x;
	}

	AStarNode * getNode(int x, int y)
	{
		return mMapVect[getIndex(x,y)];
	}
	/** 
	* @brief findNearNode 查询所有相邻的节点，包含的是已经加入开启队列的和还没有加入开启队列的
	* 
	* Detailed description.
	* @param[in] nearNodes 
	* @param[in] parent 
	*/
	void findNearNode(AStarList& nearNodes, AStarNode *parent)
	{
		//  x - 1, x, x + `1,   y - 1, y, y + 1
		int x = parent->x;
		int y = parent->y;

		// left right up down

		// left
		bool isleft = canUse(x - 1, y);
		bool isright = canUse(x + 1, y);
		bool isup = canUse(x, y - 1);
		bool isdown = canUse(x, y + 1);

		if (isleft)
		{
			int tmpIndex = getIndex(x - 1, y);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}

		// right
		if (isright)
		{
			int tmpIndex = getIndex(x + 1, y);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}

		// up
		if (isup)
		{
			int tmpIndex = getIndex(x, y - 1);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}

		// down

		if (isdown)
		{
			int tmpIndex = getIndex(x, y + 1);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}


		// lu ld ru rd
		bool islu = canUse(x - 1, y - 1);
		bool isld = canUse(x - 1, y + 1);
		bool isru = canUse(x + 1, y - 1);
		bool isrd = canUse(x + 1, y + 1);

		// left up

		if (isleft && isup && islu)
		{
			int tmpIndex = getIndex(x - 1, y - 1);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}
		
		// left down
		
		if (isleft && isdown && isld)
		{
			int tmpIndex = getIndex(x - 1, y + 1);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}

		
		// right up
		if (isright && isup && isru)
		{
			int tmpIndex = getIndex(x + 1, y - 1);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}
		
		// right down
		if (isright && isdown && isrd)
		{
			int tmpIndex = getIndex(x + 1, y + 1);
			nearNodes.push_back(mMapVect[tmpIndex]);
		}
	}	

	/** 
	* @brief addToOpenList 加入开启列表
	* 
	* Detailed description.
	* @param[in] src 
	*/
	void addToOpenList(AStarList & src)
	{
		AStarList::iterator iter = src.begin();
		for (; iter != src.end(); ++iter)
		{
			mOpenList.push_back(*iter);
		}
	}

	void addToCloseList(AStarNode *node)
	{
		mCloseList.push_back(node);
	}

	/** 
	* @brief findInOpenListAndUnInOpenList  查询列表找出已经加入开启列表的和新加入开启列表的
	* 
	* Detailed description.
	* @param[in] inList 已经加入开启列表
	* @param[in] uninlist 新加入开启列表的
	*/
	void findInOpenListAndUnInOpenList(AStarList& src, AStarList& inList, AStarList& uninlist)
	{
		AStarList::iterator iter = src.begin();
		for (; iter != src.end(); ++iter)
		{
			if (inOpenList((*iter)))
				inList.push_back(*iter);
			else
				uninlist.push_back(*iter);
		}
	}

	//-----------------------------------------------------------------------------
	// 判断是否在开启列表中
	bool inOpenList(AStarNode* k)
	{
		AStarList::iterator iter = mOpenList.begin();
		for (; iter != mOpenList.end(); ++iter)
		{
			if ((*iter) == k)
				return true;
		}
		return false;
	}
	
	//-----------------------------------------------------------------------------
	//
	bool inCloseList(AStarNode* k)
	{
		AStarList::iterator iter = mCloseList.begin();
		for (; iter != mCloseList.end(); ++iter)
		{
			if ((*iter) == k)
				return true;
		}
		return false;
	}

	//-----------------------------------------------------------------------------
	// 计算 这里计算的是新加入打开列表的节点
	void calculateF(AStarNode *parent, AStarList& newopennode)
	{

		AStarList::iterator iter = newopennode.begin();
		for (; iter != newopennode.end(); ++iter)
		{
			AStarNode *tmpNode = (*iter);
			tmpNode->parent = parent;
			calculateG(tmpNode);
			calculateH(tmpNode);
			tmpNode->f = tmpNode->h + tmpNode->g;

		}
	}
	
	
	/** 
	* @brief calculateG 计算G值,这里需要判断是父节点的方向
	* 
	* Detailed description.
	* @param[in] node 
	*/
	void calculateG(AStarNode *node)
	{
		assert(node->parent !=  0);

		AStarNode *tmpParent = node->parent;
		
		int tmpResult = abs(tmpParent->x - node->x) + abs(tmpParent->y - node->y);
		if (tmpResult > 1)
			node->g = 14.14 + tmpParent->g;
		else
			node->g = 10 + tmpParent->g;
		
	}
	/** 
	* @brief calculateH 计算H值
	* 
	* Detailed description.
	* @param[in] node 
	*/
	void calculateH(AStarNode *node)
	{
		node->h = (abs(node->x - mEndNode->x) + abs(node->y - mEndNode->y)) * 10;
	}

protected:
private:
};


int main(int argc, char **argv)
{
	//cout<<"请输入 起始点和结束点位置(注意从0开始)"<<endl;
	AStar tmpAStar;
	tmpAStar.test1();
	
	system("pause");
	return 0;
}